#include "PoseSolver.h"

/**
 * @brief 姿态解算器执行函数
 * @param  frame    rgb图像
 * @param  depth    深度图像
 * @param  intri    相机内参
 * @param  rect     目标ROI区域
 */
void PoseSolver::resolute(const cv::Mat &frame, const cv::Mat &depth, const rs2_intrinsics &intri, cv::Rect rect)
{
    // 解算器初始化
    this->_solverInit(frame, depth, intri);
    // 准备数据
    this->_prepare(frame, depth, rect);
    
    // // 点云可视化
    // this->pcdShow();
}

/**
 * @brief  姿态解算器初始化
 * @param  frame    rgb图像
 * @param  depth    深度图像
 * @param  intri    相机内参
 */
void PoseSolver::_solverInit(const cv::Mat &frame, const cv::Mat &depth, const rs2_intrinsics &intri)
{
    // mat 格式转 geometry::Image
    this->_frame = MatToImage(frame);
    this->_depth = MatToImage(depth);
    // 加载相机内参
    this->_intrinsics = this->_loadIntrinsics(intri);
}

/**
 * @brief  加载 RealSense相机内参到姿态解算器中
 * @param  intri 相机内参
 * @return camera::PinholeCameraIntrinsic
 */
camera::PinholeCameraIntrinsic PoseSolver::_loadIntrinsics(const rs2_intrinsics &intri)
{
    // 成像宽高
    const int &w = intri.width;
    const int &h = intri.height;
    // 像素宽度焦距
    const float &fx = intri.fx;
    // 像素高度焦距
    const float &fy = intri.fy;
    // 图像主点坐标，从左边缘偏移的像素
    float ppx = intri.ppx;
    float ppy = intri.ppy;

    return camera::PinholeCameraIntrinsic(w, h, fx, fy, ppx, ppy);
}

/**
 * @brief 准备数据
 * @param  rect 目标ROI区域
 */
void PoseSolver::_prepare(const cv::Mat &frame, const cv::Mat &depth, cv::Rect rect)
{
    // 矩形拓展
    cv::Rect exRect = this->_rectExpand(rect);
    this->_RgbRoiMat = frame(exRect);
    this->_DepRoiMat = depth(exRect);
    // std::cout << this->_DepRoiMat << std::endl;
    cv::imwrite("/home/mini/图片/Temp/temp6/rgb.png", frame);
    cv::imwrite("/home/mini/图片/Temp/temp6/dep.png", depth);
    // Mat 转 Image 格式
    this->_rgbRoiImage = MatToImage(_RgbRoiMat);
    this->_depRoiImage = MatToImage(_DepRoiMat);
    // 生成RGBD图像
    this->_rgbd = geometry::RGBDImage::CreateFromColorAndDepth(this->_rgbRoiImage, this->_depRoiImage, 1000.0, 4.0, false);
    // 生成点云
    this->_pcd = geometry::PointCloud::CreateFromRGBDImage(*this->_rgbd, this->_intrinsics);
    // // 点云旋转
    // Eigen::Matrix4d tranformMat(4, 4);
    // tranformMat << 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1;
    // this->_pcd->Transform(tranformMat);
    // // 保存点云
    // io::WritePointCloud("/home/mini/图片/Temp/temp5/pointCloud.pcd", *(this->_pcd));
}

/**
 * @brief  矩形框拓展
 * @return cv::Rect 
 */
cv::Rect PoseSolver::_rectExpand(const cv::Rect &rect)
{
    // 拓展边缘像素范围
    int Padding = (rect.width + rect.height) / 2 / 20 / 2;
    // 矩形扩展
    int w = rect.width + 2 * Padding;
    int h = rect.height + 2 * Padding;
    int x = rect.x - Padding;
    int y = rect.y - Padding;
    // 范围限制
    x = x < 0 ? 0 : x;
    y = y < 0 ? 0 : y;
    w = (x + w > this->_frame.width_) ? this->_frame.width_ - x - 1 : w;
    h = (y + h > this->_frame.height_) ? this->_frame.height_ - y - 1 : h;
    return cv::Rect(x, y, w, h);
}

/**
 * @brief 点云可视化
 */
void PoseSolver::pcdShow()
{
    // 创建虚拟窗口
    this->CreateVisualizerWindow("Open3D", this->_intrinsics.width_, this->_intrinsics.height_);
    // 添加影像流
    this->UpdateGeometry(this->_pcd);
    // 显示窗口
    this->Run();
}

/**
 * @brief  OpenCv Mat 转 Open3D geometry::Image
 * @param  mat  OpenCv 中的图片存储格式
 * @return geometry::Image Open3D中的图片存储格式
 */
geometry::Image MatToImage(const cv::Mat &mat)
{
    geometry::Image image;
    int bytes = (mat.depth() / 2 + 1);
    image.Prepare(mat.cols, mat.rows, mat.channels(), bytes);
    std::memcpy(image.data_.data(), mat.data, mat.total() * mat.channels() * bytes);
    return image;
}

/**
 * @brief  Open3D geometry::Image 转 OpenCv Mat
 * @param  image   Open3D中的图片存储格式
 * @return cv::Mat OpenCv 中的图片存储格式
 */
cv::Mat ImageToMat(const geometry::Image &image)
{
    int type = image.num_of_channels_ == 1 ? CV_16UC1 : CV_8UC3;
    cv::Mat mat = cv::Mat(image.height_, image.width_, type, (void *)image.data_.data());
    return mat;
}
